/*
 * block_handler.c
 *
 * Copyright 2011 Pieter Agten
 *
 * This file is part of Yarf.
 *
 * Yarf is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Yarf is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Yarf.  If not, see <http://www.gnu.org/licenses/>.
 */

/**
 * @file   block_handler.c
 * @author Pieter Agten (pieter.agten@gmail.com)
 * @date   23 sep 2011
 * @brief  The block handler component is responsible for executing blocks 
 *         placed into the planning queue by the planner component.
 */

#include "block_handler.h"

#include "yarf.h"
#include "movement/planner_queue.h"
#include "hardware/steppers.h"
#include "hardware/endstops.h"
#include "hardware/usart.h"
#include "scheduling/realtime_timer.h"
#include "util/math.h"
#include "advance.h"

#include <stdlib.h>
#include <util/delay.h>
#include <util/atomic.h>

/**
 * The number of timer ticks to pass as the delay when starting the block
 * handler thread.
 */
#define BLOCK_HANDLER_START_DELAY_TICKS 1L

/**
 * Indicates whether the block handler thread is running.
 */
static bool handler_running;

/**
 * Structure representing the block handler task. This structure is passed to
 * the real-time timer, so it will know what function to execute when the timer
 * rings.
 */
static realtime_task_t handler_task;


/**
 * Pointer to the block currently being executed.
 */
static block_t *b;


/**
 * Counter for each axis, used to calculate whether an axis' stepper motor
 * needs to take a step in the \a block_handler_run procedure. 
 */
static steps_t counter[NUM_AXES]; 


#if ADVANCE_ALGORITHM
/**
 * The number of steps the extruder needs to take. Only used if the advance
 * pressure management system is used.
 */
static volatile int32_t e_steps;

/**
 * The previous number of advance steps.
 *
 * Used to calculate the difference between the previous number of advance
 * steps and the current number of advance steps, in order to know how many
 * steps to add to the \a e_steps variable.
 */
static int32_t previous_advance;

/**
 * State of the advance pressure management extruder thread.
 *
 * The extruder thread can be in one of two states: low or high. In the low 
 * state, the next time the extruder thread executes, the extruder step signal
 * will be taken low. In the high state, the extruder step signal will be taken
 * high if the extruder needs to take a step.
 */
static uint8_t extruder_tick_state;
#endif

/**
 * Initializes the \a bp variable. Called whenever a new block is taken from
 * the planning queue to be executed.
 */
static void
block_progress_init(void)
{
  int axis;
  for(axis = 0; axis < NUM_AXES; ++axis) {
    counter[axis] = -(b->nb_steps/2);
  }
}

/**
 * Enables each of the stepper motors that needs to be stepped for the current
 * block. Called whenever a new block is taken from the planning queue to be
 * executed.
 */
static inline void
enable_steppers(void)
{
  if (b->steps[X_AXIS] != 0) {
    stepper_enable_x();
  }
  if (b->steps[Y_AXIS] != 0) {
    stepper_enable_y();
  }
  if (b->steps[Z_AXIS] != 0) {
    stepper_enable_z();
  }
  if (b->steps[E_AXIS] != 0) {
    stepper_enable_e();
  }
}

/**
 * Disables each of the stepper motors that need not be held after executing a
 * block.
 */
static inline void
disable_steppers(void)
{
#if !STEPPER_HOLD_X
  stepper_disable_x();
#endif
#if !STEPPER_HOLD_Y
  stepper_disable_y();
#endif
#if !STEPPER_HOLD_Z
  stepper_disable_z();
#endif
#if !STEPPER_HOLD_E && !ADVANCE_ALGORITHM
  stepper_disable_e();
#endif
}

/**
 * Cleans up some data structures after finishing the execution of a block.
 */
static inline void
block_finished(void)
{
  b = NULL;

  /* Remove the block from the tail of the queue */
  plan_q_discard_tail();
  
  /* Disable the stepper motors that need not be held */
  disable_steppers();
}

/**
 * Cancels the execution of the current block in execution.
 */
static inline void
cancel_block(void)
{
  block_finished();
  rttimer_schedule_ticks(&handler_task,1);
}


#if ADVANCE_ALGORITHM
//TODO: enable/disable interrupt

/**
 * The advance pressure management extruder thread procedure.
 * 
 * If the advance pressure management system is enabled, this procedure is 
 * executed periodically in a separate thread. It will set the extruder stepper
 * motor high or low depending on the \a extruder_tick_state and on the value
 * of \a e_steps. 
 */
static inline void
block_handler_extruder_tick(void)
{
  /* Set the timer 1 A top value */
  OCR1A += EXTRUDER_TIMER_TICKS;
  TIFR1 |= _BV(OCF1A);

  /* Check the USART0 receive buffer, to prevent data overrun */
  usart0_try_read_character();

  if (extruder_tick_state == 0) {
    extruder_tick_state = 1;
    stepper_e_step_low();
  } else {
    extruder_tick_state = 0;
      if (e_steps == 0) {
        return;
      }
      
      if (e_steps < 0) {
        stepper_e_dir_neg();
        e_steps += 1;
      } else if (e_steps > 0) {
        stepper_e_dir_pos();
        e_steps -= 1;
      }
    stepper_e_step_high();
  }  
}

/**
 * Interrupt service routine for the advance pressure management thread
 * procedure, for controlling the extruder stepper motor.
 */
ISR(TIMER1_COMPA_vect, ISR_BLOCK)
{
  block_handler_extruder_tick();
}

#endif


/**
 * Updates the current block's advance field, if the advance pressure management
 * system is enabled.
 */
static inline void
update_advance(void)
{
#if ADVANCE_ALGORITHM
  b->advance = advance_get_value(b);
#endif
}

/**
 * Main block handler implementation.
 *
 * This procedure is called from a seperate thread (an interrupt) whenever the
 * real-time timer rings. It makes the stepper motors take one linear step of
 * the current block on each call. If there is no current block, it first tries
 * to get one from the planning queue. It calculates which stepper motors need
 * to be stepped on each call. After stepping those stepper motors, the
 * procedure calculates how many timer ticks need to pass before the step must
 * occur and reschedules the real-time timer accordingly. The number of timer
 * ticks between steps changes during acceleration and deceleration.
 *
 * Before this procedure steps any one of the stepper motors, it checks whether
 * the end stop of that motor in the specified direction is pressed and, if so,
 * cancels the block being executed and calls the \a collision_handler function
 * pointer of the block.
 */
static void
block_handler_run(void)
{
  if (b == NULL) {
    /* Get new block to execute from the planning queue */
    b = plan_q_tail();
    if (b == NULL) {
      /* No blocks to be executed */
      handler_running = false;
      return;
    } else {
      block_progress_init();
      enable_steppers();
    }
  }


  /* Make sure all the stepper inputs are low */
  stepper_x_step_low();
  stepper_y_step_low();
  stepper_z_step_low();
#if !ADVANCE_ALGORITHM
  stepper_e_step_low();
#endif

  
  /* Setup the steppers' direction pins and check the endstops */
  uint8_t collisions = 0;
  if (b->steps[X_AXIS] < 0) {
    stepper_x_dir_neg();
    if (endstop_x_min()) {
      collisions |= _BV(X_AXIS_NEG);
    }
  } else if (b->steps[X_AXIS] > 0) {
    stepper_x_dir_pos();
    if (endstop_x_max()) {
      collisions |= _BV(X_AXIS_POS);
    }
  }
  if (b->steps[Y_AXIS] < 0) {
    stepper_y_dir_neg();
    if (endstop_y_min()) {
      collisions |= _BV(Y_AXIS_NEG);
    }
  } else if (b->steps[Y_AXIS] > 0) {
    stepper_y_dir_pos();
    if (endstop_y_max()) {
      collisions |= _BV(Y_AXIS_POS);
    }
  }
  if (b->steps[Z_AXIS] < 0) {
    stepper_z_dir_neg();
    if (endstop_z_min()) {
      collisions |= _BV(Z_AXIS_NEG);
    }
  } else if (b->steps[Z_AXIS] > 0) {
    stepper_z_dir_pos();
    if (endstop_z_max()) {
      collisions |= _BV(Z_AXIS_POS);
    }
  }
#if !ADVANCE_ALGORITHM
  if (b->steps[E_AXIS] < 0) {
    stepper_e_dir_neg();
  } else {
    stepper_e_dir_pos();
  }
#endif

  if (collisions) {
    b->collision_handler(collisions);
    cancel_block();
    return;
  }

  /* Step all the steppers that need to step */
  counter[X_AXIS] += labs(b->steps[X_AXIS]);
  if (counter[X_AXIS] > 0) {
    counter[X_AXIS] -= b->nb_steps;
    stepper_x_step_high();
  }
  counter[Y_AXIS] += labs(b->steps[Y_AXIS]);
  if (counter[Y_AXIS] > 0) {
    counter[Y_AXIS] -= b->nb_steps;
    stepper_y_step_high();
  }
  counter[Z_AXIS] += labs(b->steps[Z_AXIS]);
  if (counter[Z_AXIS] > 0) {
    counter[Z_AXIS] -= b->nb_steps;
    stepper_z_step_high();
  }
#if !ADVANCE_ALGORITHM
  counter[E_AXIS] += labs(b->steps[E_AXIS]);
  if (counter[E_AXIS] > 0) {
    counter[E_AXIS] -= b->nb_steps;
    stepper_e_step_high();
  }
#else
  /* Calculate E steps to take */
  counter[E_AXIS] += labs(b->steps[E_AXIS]);
  if (counter[E_AXIS] > 0) {
    counter[E_AXIS] -= b->nb_steps;
    if (b->steps[E_AXIS] < 0) {
      e_steps -= 1;
    } else {
      e_steps += 1;
    }
  }    
  /* Do advance steps */
  e_steps += (b->advance - previous_advance);
  previous_advance = b->advance;
  block_handler_extruder_tick();
#endif



  /* Reschedule the timer */
  b->nb_steps_completed += 1;
  fxp16u16_t diff = fxp16u16_div_uint16(2*b->timer_ticks, b->acceleration_step);
  if (b->nb_steps_completed < b->acceleration_end) {
    /* We're in the acceleration slope */
    if (b->timer_ticks - b->plateau_timer_ticks > diff) { // overflow detection
      b->timer_ticks -= diff;
    } else {
      b->timer_ticks = b->plateau_timer_ticks;
    }

    b->acceleration_step += 4;
    update_advance();
  } else if (b->nb_steps_completed >= b->deceleration_start) {
    /* We're in the deceleration slope */
    if (b->timer_ticks < b->exit_timer_ticks &&
        b->exit_timer_ticks - b->timer_ticks > diff) { // overflow detection
      b->timer_ticks += diff; 
    } else {
      b->timer_ticks = b->exit_timer_ticks;
    }

    if (b->acceleration_step > 4) {
      b->acceleration_step -= 4;
    } else {
      b->acceleration_step = 1;
    }
    update_advance();
  } else {
    b->timer_ticks = b->plateau_timer_ticks;
  }
  rttimer_schedule_ticks(&handler_task, fxp16u16_to_uint16(b->timer_ticks));


  /* Check if we're done with this block */
  if (b->nb_steps_completed >= b->nb_steps) {
    block_finished();
  }
}



void
block_handler_init(void)
{
  rttimer_init();

  b = NULL;
  handler_running = false;
  handler_task.f = block_handler_run;

#if ADVANCE_ALGORITHM
  e_steps = 0;
  previous_advance = 0;
  extruder_tick_state = 0;
#endif
}

void 
block_handler_start(void)
{
  if (! handler_running) {
    handler_running = true;
    rttimer_schedule_ticks(&handler_task, BLOCK_HANDLER_START_DELAY_TICKS);
  }
}

void
block_handler_pause(void)
{
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if (handler_running) {
      rttimer_cancel();
      handler_running = false;    
    }
  }
}

void
block_handler_stop(void)
{
  ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
    if (handler_running) {
      rttimer_cancel();
      if (b != NULL) {
        block_finished();
      }
      handler_running = false;    
    }
  }
}

bool
block_handler_running(void)
{
  return handler_running;
}
